#include "PelcoD.h"

#define SET_BIT(p,n) ((p) |= (1 << (n)))

#define CLR_BIT(p,n) (p &= ~(1 << n))

//构造函数
PelcoD::PelcoD()
{
	// clear();
	// data[0] = 0xFF;
	// data[1] = addr;
}
//初始化函数
int PelcoD::PelcoD_init(uint8_t addr)
{
	clear();
	PelcoD_data[0] = 0xFF;
	PelcoD_data[1] = addr;

	return 0;
}

//清除函数
void PelcoD::clear()
{
	PelcoD_data[2] = 0;
	PelcoD_data[3] = 0;
	PelcoD_data[4] = 0;
	PelcoD_data[5] = 0;
	PelcoD_data[6] = 0;
}

//上移
void PelcoD::moveUp(uint8_t speed)
{
	CLR_BIT(PelcoD_data[3], 4);
	SET_BIT(PelcoD_data[3], 3);
	if(speed > 0x3f) speed = 0x3f;
	PelcoD_data[5] = speed;
}

//下移
void PelcoD::moveDown(uint8_t speed)
{
	CLR_BIT(PelcoD_data[3], 3);
	SET_BIT(PelcoD_data[3], 4);
	if(speed > 0x3f) speed = 0x3f;
	PelcoD_data[5] = speed;
}

//左移
void PelcoD::moveLeft(uint8_t speed)
{
	CLR_BIT(PelcoD_data[3], 1);
	SET_BIT(PelcoD_data[3], 2);
	PelcoD_data[4] = speed;
}

//右移
void PelcoD::moveRight(uint8_t speed)
{
	CLR_BIT(PelcoD_data[3], 2);
	SET_BIT(PelcoD_data[3], 1);
	PelcoD_data[4] = speed;
}

//设置预设
void PelcoD::setPreset(uint8_t preset)
{
	PelcoD_data[2] = 0;
	PelcoD_data[3] = 3;
	PelcoD_data[4] = 0;
	// if(preset > 20) preset = 20; //only 20 presets allowed according to doc
	PelcoD_data[5] = preset;
}

void PelcoD::getPreset(uint8_t preset){
	PelcoD_data[2] = 0;
	PelcoD_data[3] = 7;
	PelcoD_data[4] = 0;
	PelcoD_data[5] = preset;
}

void PelcoD::delPreset(uint8_t preset){
	PelcoD_data[2] = 0;
	PelcoD_data[3] = 5;
	PelcoD_data[4] = 0;
	PelcoD_data[5] = preset;
}

//计算校验和
void PelcoD::calcChecksum()
{
	uint16_t sum = 0;
	for(int i = 1; i < 6; ++i)
	{
		sum += PelcoD_data[i];
	}
	sum = sum % 256;
	PelcoD_data[6] = (uint8_t)sum;
}
